﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Microsoft.Devices.Sensors;
using Microsoft.Xna.Framework;
using System.Threading;
using StaticView.Input;
using S = StaticView.Algorithm;

namespace StaticView
{
    // Static Position class
    //      Contain Postion thread process
    //      Store the position of the phone
    static class Position
    {
        public static Vector3 Normal { get; private set; } // Normal vector (unit) of the plane (local z)
        public static Vector3 LocalX { get; private set; } // Local x vector (unit) of the phone
        public static Vector3 Coordinates { get; private set; } // Coordinates of the phone in 3D

        static List<ISensor> sensors; // List of sensors
        static volatile bool stop = false;

        // Calibration variables
        static float gravity = 0;
        static int nosensors = 0; // Number of calibrated sensors
        static int noacce = 0; // Number of calibrated accelerometer
        static S.Matrix noise;
        static Vector3 grav = new Vector3();
        static MainpageCallback callback;
        static List<Thread> calThreads;

        // Constants
        const int CALTIME = 10000; // Calibrating time

        // Calibrate the position (called at the Mainpage)
        public static void Calibrate(MainpageCallback callback)
        {
            Position.callback = callback;

            Normal = new Vector3(0, 0, 1); // Local vector will be calibrated later
            LocalX = new Vector3(1, 0, 0);
            Coordinates = new Vector3(0, 0, 0); // Initial coordinates

            sensors = new List<ISensor>();
            sensors.Add(new InternalAccelerometer(SensorCallback, 0));

            calThreads = new List<Thread>(); // List of calibrating threads
            for (int i = 0; i < sensors.Count; i++)
            {
                Thread t =new Thread(new ParameterizedThreadStart(sensors[i].Calibrate));
                t.Start(CALTIME);
                calThreads.Add(t);
            }

            Thread preThread = new Thread(new ThreadStart(PreparationProcess));
            preThread.Start();
        }

        // Sensor calibration callback, use to send back the data
        public static void SensorCallback(int id, float[] data, S.Matrix noise, Vector3 grav)
        {
            nosensors++;
            Position.noise = noise;
            Position.grav = grav;
            // Calculate the gravity
            if (sensors[id].Type == SensorType.Accelerometer) {
                noacce++;
                gravity += data[0];
            }
        }

        // Preparation thread process, process after all calibration threads are finished
        static void PreparationProcess()
        {
            // Wait until all threads are finished
            foreach (Thread t in calThreads)
                t.Join();

            // Preparation
            gravity /= noacce;

            // Callback to Mainpage
            callback(gravity, noise.ToString(), grav.ToString());
        }

        // Update the position (start thread in Mainpage)
        // Use Kalman filter
        public static void Update() {
            while (!stop)
            {
                Thread.Sleep(1000);
            }
        }

        public static void Stop()
        {
            stop = true;
            foreach (ISensor sensor in sensors)
                sensor.Stop();
        }
    }

    // Callback with parameter (test only)
    delegate void MainpageCallback(float gravity, string noise, string grav);
}
